/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : freertos.c
  * Description        : Code for freertos applications
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2025 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */

/* Includes ------------------------------------------------------------------*/
#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "ads1115.h"
#include "RobotControl.h"
#include "tim.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

#define hand GPIOB
#define arm GPIOC
#define leg GPIOF

#define hand_PIN GPIO_PIN_0         //夹爪
#define arm_PIN GPIO_PIN_4          //投篮推开的机构
#define leg_PIN GPIO_PIN_8          //击打气缸

#define MAXSPEED 2000
//在最低点启动电机
//#define HIGH_POSITION -1130000
//#define MID_POSITION -1095000
#define HIGH_POSITION 185000        //升降电机最高点的位置
#define MID_POSITION  90000         //升降电机中间点的位置
#define LOW_POSITION 0              //升降电机最低点的位置

#define MAX_ACCEL_UP_VX1   1.0f*2     // X方向加速时最大加速度 (绝对值增大)
#define MAX_ACCEL_DOWN_VX1 2.5f*2     // X方向减速时最大加速度 (绝对值减小)
#define MAX_ACCEL_UP_VY1   1.0f*2     // Y方向加速时最大加速度 (绝对值增大)
#define MAX_ACCEL_DOWN_VY1 2.5f*2     // Y方向减速时最大加速度 (绝对值减小)
#define MAX_ACCEL_UP_VW1   1.0f*2     //角速度加速时最大加速度 (绝对值增大)
#define MAX_ACCEL_DOWN_VW1 2.5f*2     //角速度减速时最大加速度 (绝对值减小)

/********自动模式下的跑点模式********/
struct Auto_Mode{
/*
 *throw_mode = 1时投篮模式，不用时置0
 *hitting_ball_mode = 1时运球模式，不用时置0
 *back_origin = 1时回边界模式，不用时置0
 *lazer_mode =  1时激光纠正点位，不用时置0
 *
*/
    uint8_t throw_mode;
    uint8_t hitting_ball_mode;
    uint8_t back_origin;
    uint8_t lazer_mode;
    uint8_t dingdian_mode;
}Auto_Mode;
/***********************************/  

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN Variables */

/*
*Change_N task:根据手柄状态增加出手速度
*Change_yaw task:根据手柄状态偏转yaw值
*StartDefault task：根据手柄状态调节升降电机
*Start  task02：M3508的速度解算
*AK7010 task：根据手柄状态确定运行模式并发送数据给A板
*M3508  task：M3508电流环
*AK7010_reinit：电机重新上锁
*/

/*
*控制抬升电机：SE - > CH11
*控制自动，手动：SA - > CH5
*控制投篮，运球，回边界：SB -> CH6
*控制投篮动作：SG -> CH13
*控制微调yaw值：SC -> CH9
*控制投出力度：SD -> CH7
*/

/*******************************************************/
uint8_t current_CH5_state = 0;
uint8_t last_CH5_state = 0;

uint8_t current_CH10_state = 0;
uint8_t last_CH10_state = 0;

uint8_t steps = 0;

int8_t gear = 0;

/*********************淘汰赛定点***********************/
uint8_t jiguang_cmd = 0;  //激光使能标记位
uint8_t index = 0;        //淘汰赛定点角标
float dingdian_x[8] = {0,};
float dingdian_y[8] = {0,};
float dingdian_yaw[8] = {0,};
extern uint8_t index_temp;/*用于将手柄的index传给底盘*/
/*******************************************************/
uint8_t yunqiu_flag = 0; //手动运球标志位
uint8_t jiaozhun_flag = 0;//手动校准标志位




/*****激光标定的最后一个投篮点的点位*****/
float lazer_throw_x = 0;
float lazer_throw_y = 0;
float lazer_throw_yaw = 0;
/****************************************/

/*********用于存储手动纠正后的点位***********/
float extra_yaw = 0;           /*需要额外偏转的yaw*/
float extra_Tf = 0;            /*需要增加的额外力矩*/
float extra_angle = 0;         /*需要额外增加的角度*/
uint8_t get_dingdian_flag = 0; /*完成标定的标志位*/
/**********************************************/

unsigned char error_flag = 0;

/***************激光的定位参数****************/
float laser_x=0;
float laser_y=0;
float X_Voltage = 0;
float Y_Voltage = 0;
/**********************************************/

/**********************************************/
typedef union/*码盘发送数据给A板*/
{
  float float_Buffer[5];
  uint8_t bytes[20];
}Data;
Data SendBuffer; 

uint8_t Tx_buffer[26] = {0,};//给A板发送数据的缓冲区
uint8_t AK7010_flag = 0;     //控制单臂标志位
uint8_t reinit_flag = 0;     //电机重启标志位
/***********************************************/


/****************************/





/* USER CODE END Variables */
/* Definitions for defaultTask */
osThreadId_t defaultTaskHandle;
const osThreadAttr_t defaultTask_attributes = {
  .name = "defaultTask",
  .stack_size = 128 * 4,
  .priority = (osPriority_t) osPriorityNormal,
};
/* Definitions for Throwing_Ball */
osThreadId_t Throwing_BallHandle;
const osThreadAttr_t Throwing_Ball_attributes = {
  .name = "Throwing_Ball",
  .stack_size = 128 * 4,
  .priority = (osPriority_t) osPriorityNormal,
};
/* Definitions for myTask03 */
osThreadId_t myTask03Handle;
const osThreadAttr_t myTask03_attributes = {
  .name = "myTask03",
  .stack_size = 128 * 4,
  .priority = (osPriority_t) osPriorityNormal,
};
/* Definitions for myTask04 */
osThreadId_t myTask04Handle;
const osThreadAttr_t myTask04_attributes = {
  .name = "myTask04",
  .stack_size = 128 * 4,
  .priority = (osPriority_t) osPriorityNormal,
};
/* Definitions for Speedtask */
osThreadId_t SpeedtaskHandle;
const osThreadAttr_t Speedtask_attributes = {
  .name = "Speedtask",
  .stack_size = 128 * 4,
  .priority = (osPriority_t) osPriorityNormal,
};
/* Definitions for Sbustask */
osThreadId_t SbustaskHandle;
const osThreadAttr_t Sbustask_attributes = {
  .name = "Sbustask",
  .stack_size = 128 * 4,
  .priority = (osPriority_t) osPriorityNormal,
};
/* Definitions for ADS1115 */
osThreadId_t ADS1115Handle;
const osThreadAttr_t ADS1115_attributes = {
  .name = "ADS1115",
  .stack_size = 128 * 4,
  .priority = (osPriority_t) osPriorityLow,
};
/* Definitions for change_yaw */
osThreadId_t change_yawHandle;
const osThreadAttr_t change_yaw_attributes = {
  .name = "change_yaw",
  .stack_size = 128 * 4,
  .priority = (osPriority_t) osPriorityLow,
};
/* Definitions for change_N */
osThreadId_t change_NHandle;
const osThreadAttr_t change_N_attributes = {
  .name = "change_N",
  .stack_size = 128 * 4,
  .priority = (osPriority_t) osPriorityLow,
};
/* Definitions for ak7010_reinit */
osThreadId_t ak7010_reinitHandle;
const osThreadAttr_t ak7010_reinit_attributes = {
  .name = "ak7010_reinit",
  .stack_size = 128 * 4,
  .priority = (osPriority_t) osPriorityLow,
};
/* Definitions for CH14_to_hand_yu */
osThreadId_t CH14_to_hand_yuHandle;
const osThreadAttr_t CH14_to_hand_yu_attributes = {
  .name = "CH14_to_hand_yu",
  .stack_size = 128 * 4,
  .priority = (osPriority_t) osPriorityLow,
};
/* Definitions for change_angle */
osThreadId_t change_angleHandle;
const osThreadAttr_t change_angle_attributes = {
  .name = "change_angle",
  .stack_size = 128 * 4,
  .priority = (osPriority_t) osPriorityLow,
};

/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */
void Connect_to_F405();
void yunqiu(void);

/* USER CODE END FunctionPrototypes */

void StartDefaultTask(void *argument);
void StartTask02(void *argument);
void AK7010_Task(void *argument);
void M3508_Task(void *argument);
void speedtask(void *argument);
void sbustask(void *argument);
void ads1115(void *argument);
void Change_yaw(void *argument);
void Change_N(void *argument);
void Ak7010_reinit(void *argument);
void ch14_to_hand_yunqiu(void *argument);
void Change_angle(void *argument);

void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */

/**
  * @brief  FreeRTOS initialization
  * @param  None
  * @retval None
  */
void MX_FREERTOS_Init(void) {
  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* USER CODE BEGIN RTOS_MUTEX */
  /* add mutexes, ... */
  /* USER CODE END RTOS_MUTEX */

  /* USER CODE BEGIN RTOS_SEMAPHORES */
  /* add semaphores, ... */
  /* USER CODE END RTOS_SEMAPHORES */

  /* USER CODE BEGIN RTOS_TIMERS */
  /* start timers, add new ones, ... */
  /* USER CODE END RTOS_TIMERS */

  /* USER CODE BEGIN RTOS_QUEUES */
  /* add queues, ... */
  /* USER CODE END RTOS_QUEUES */

  /* Create the thread(s) */
  /* creation of defaultTask */
  defaultTaskHandle = osThreadNew(StartDefaultTask, NULL, &defaultTask_attributes);

  /* creation of Throwing_Ball */
  Throwing_BallHandle = osThreadNew(StartTask02, NULL, &Throwing_Ball_attributes);

  /* creation of myTask03 */
  myTask03Handle = osThreadNew(AK7010_Task, NULL, &myTask03_attributes);

  /* creation of myTask04 */
  myTask04Handle = osThreadNew(M3508_Task, NULL, &myTask04_attributes);

  /* creation of Speedtask */
  SpeedtaskHandle = osThreadNew(speedtask, NULL, &Speedtask_attributes);

  /* creation of Sbustask */
  SbustaskHandle = osThreadNew(sbustask, NULL, &Sbustask_attributes);

  /* creation of ADS1115 */
  ADS1115Handle = osThreadNew(ads1115, NULL, &ADS1115_attributes);

  /* creation of change_yaw */
  change_yawHandle = osThreadNew(Change_yaw, NULL, &change_yaw_attributes);

  /* creation of change_N */
  change_NHandle = osThreadNew(Change_N, NULL, &change_N_attributes);

  /* creation of ak7010_reinit */
  ak7010_reinitHandle = osThreadNew(Ak7010_reinit, NULL, &ak7010_reinit_attributes);

  /* creation of CH14_to_hand_yu */
  CH14_to_hand_yuHandle = osThreadNew(ch14_to_hand_yunqiu, NULL, &CH14_to_hand_yu_attributes);

  /* creation of change_angle */
  change_angleHandle = osThreadNew(Change_angle, NULL, &change_angle_attributes);

  /* USER CODE BEGIN RTOS_THREADS */
  /* add threads, ... */
  /* USER CODE END RTOS_THREADS */

  /* USER CODE BEGIN RTOS_EVENTS */
  /* add events, ... */
  /* USER CODE END RTOS_EVENTS */

}

/* USER CODE BEGIN Header_StartDefaultTask */
/**
  * @brief  Function implementing the defaultTask thread.
  * @param  argument: Not used
  * @retval None
  */
/* USER CODE END Header_StartDefaultTask */
void StartDefaultTask(void *argument)
{
  /* USER CODE BEGIN StartDefaultTask */
   /*
   *该任务块专门用于解析SA的值，
   *用于控制升降电机
   */
  /* Infinite loop */
  for(;;)
  {
      /*******************解析通道11，使用SA控制*****************/
      switch(SBUS_Analyse_Switch(SBUS.CH11))
      {
        /*-100->case0*/case 0://机器人上电时即处于的模式
            M3508[5].Exparg=LOW_POSITION;    
            osDelay(10);
            HAL_GPIO_WritePin (arm,arm_PIN,GPIO_PIN_RESET);//防止夹爪打开
        break;
        
        /*0->case1*/case 1://机器人运球模式
            M3508[5].Exparg=MID_POSITION;                  //升降电机处于中间位置
            osDelay(10);  
            HAL_GPIO_WritePin (arm,arm_PIN,GPIO_PIN_RESET);//防止夹爪打开
            /*********************************************************/
        break;
      
        /*+100->2*/case 2://机器人投篮模式
            M3508[5].Exparg=HIGH_POSITION;
            HAL_GPIO_WritePin (arm,arm_PIN,GPIO_PIN_SET);    //上层推臂张开
            osDelay(100);
            HAL_GPIO_WritePin (leg,leg_PIN,GPIO_PIN_RESET);  //防止气缸击打
            osDelay(100);
            HAL_GPIO_WritePin (hand,hand_PIN,GPIO_PIN_RESET);//防止夹爪张开
            osDelay(10);      
        break;
     }
     
     if(__HAL_TIM_GET_COUNTER(&htim5) > 800000 && claw_flag)
     {
        HAL_GPIO_WritePin (hand,hand_PIN,GPIO_PIN_RESET);
        HAL_TIM_Base_Stop(&htim5);
        __HAL_TIM_SET_COUNTER(&htim5,0);
        claw_flag = 0;
     }
     osDelay(1);
  }
  /* USER CODE END StartDefaultTask */
}

/* USER CODE BEGIN Header_StartTask02 */
/**
* @brief Function implementing the Throwing_Ball thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_StartTask02 */
void StartTask02(void *argument)
{
  /* USER CODE BEGIN StartTask02 */
  /* Infinite loop */
  for (;;)
  {
    /*****************M3508的电机控制*******************/
    if ((Robot.Robot_Contorl_Mode == CONTROL_MANUAL) )
    {
      float Vx, Vy, Vw;
      Vx = Robot.Robot_in_self.velocity_exp.Vx;
      Vy = Robot.Robot_in_self.velocity_exp.Vy;
      Vw = Robot.Robot_in_self.velocity_exp.Vw;
      if (!Vx && !Vy && !Vw)
      {
        
      }
      Robot_Velocity_To_Wheel_Speed();
      Wheel_Speed_To_Motor();
    }
    
    else if ((Robot.Robot_Contorl_Mode == CONTROL_WORLD))
    {
      Robot_Velocity_World_To_Self();
      Robot_Velocity_To_Wheel_Speed();
      Wheel_Speed_To_Motor();
    }
    
    else if ((Robot.Robot_Contorl_Mode == CONTROL_AUTO))
    {
      Robot_Velocity_World_To_Self();
      Robot_Velocity_To_Wheel_Speed();
      Wheel_Speed_To_Motor();
    }
    osDelay(10);
  /***************************************************/
  }
  /* USER CODE END StartTask02 */
}

/* USER CODE BEGIN Header_AK7010_Task */
/**
* @brief Function implementing the myTask03 thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_AK7010_Task */
void AK7010_Task(void *argument)
{
  /* USER CODE BEGIN AK7010_Task */
 /*数据帧格式：
  *帧头1：0x25
  *帧头2：0x05
  *数据段：gear(1byte),AK7010_flag(1byte),reinit_flag(1byte),x(4bytes),y(4bytes),yaw(4bytes),extra_speed(4bytes)
  *帧尾：0x55
  */
  Tx_buffer[0] = 0x25;      
  Tx_buffer[1] = 0x05;
  Tx_buffer[25] = 0x55;
  uint8_t pre_gear = 100; /*上次gear的值*/
  uint8_t pre_flag = 100; /*上次flag的值*/
  uint8_t Tx_count = 0;   /*发送次数*/
  Connect_to_F405();      /*首先发送一次指令让F405进行单臂电机初始化*/
   
  /* Infinite loop */

  for(;;)
  {
      SendBuffer.float_Buffer[0] = Robot.Robot_in_self.position_now.x;
      SendBuffer.float_Buffer[1] = Robot.Robot_in_self.position_now.y;
      SendBuffer.float_Buffer[2] = Robot.Robot_in_self.position_now.yaw;
      SendBuffer.float_Buffer[3] = extra_Tf;
      SendBuffer.float_Buffer[4] = extra_angle;
      Tx_buffer[2] = gear;
      Tx_buffer[3] = AK7010_flag;
      Tx_buffer[4] = reinit_flag;
      /***************传x***************/
      Tx_buffer[5] = SendBuffer.bytes[0];
      Tx_buffer[6] = SendBuffer.bytes[1];
      Tx_buffer[7] = SendBuffer.bytes[2];
      Tx_buffer[8] = SendBuffer.bytes[3];
      /**************传y****************/
      Tx_buffer[9] = SendBuffer.bytes[4];
      Tx_buffer[10] = SendBuffer.bytes[5];
      Tx_buffer[11] = SendBuffer.bytes[6];
      Tx_buffer[12] = SendBuffer.bytes[7];
      /**************传yaw**************/
      Tx_buffer[13] = SendBuffer.bytes[8];
      Tx_buffer[14] = SendBuffer.bytes[9];
      Tx_buffer[15] = SendBuffer.bytes[10];
      Tx_buffer[16] = SendBuffer.bytes[11]; 
      /************传额外力矩***********/
      Tx_buffer[17] = SendBuffer.bytes[12];
      Tx_buffer[18] = SendBuffer.bytes[13];
      Tx_buffer[19] = SendBuffer.bytes[14];
      Tx_buffer[20] = SendBuffer.bytes[15];
      /***********传额外角度*************/
      Tx_buffer[21] = SendBuffer.bytes[16];
      Tx_buffer[22] = SendBuffer.bytes[17];
      Tx_buffer[23] = SendBuffer.bytes[18];
      Tx_buffer[24] = SendBuffer.bytes[19];
      
      switch (SBUS_Analyse_Switch(SBUS.CH13))
      {
        case -1:
        break;
          
        case 0:
          gear = index;
          Tx_buffer[2] = gear;
          AK7010_flag = 0;/*置0单臂不动*/
          Tx_buffer[3] = AK7010_flag;
        break;
          
        case 1:
        break;
        
        case 2:
          gear = index;
          Tx_buffer[2] = gear;
          AK7010_flag = 1;/*置1单臂执行投篮动作*/
          Tx_buffer[3] = AK7010_flag;
        break;
       }
      osDelay(10);
     /*当且仅当拨动旋钮和按下投篮拨杆时才会发送数据指令*/
//     if(((gear != pre_gear) || (AK7010_flag != pre_flag)))
//     {
//         /*重复发送确保A板能够接收到指令*/
//         while(Tx_count < 10)
//         {
//             Tx_count++;
//             HAL_UART_Transmit(&huart4,Tx_buffer,17,0xfff);
//             osDelay(10);
//         }
//         Tx_count = 0;
//         pre_gear = gear;
//         pre_flag = AK7010_flag;
//     }
    HAL_UART_Transmit(&huart4,Tx_buffer,26,0xfff);
    osDelay(20);
  }
  /* USER CODE END AK7010_Task */
}

/* USER CODE BEGIN Header_M3508_Task */
/**
* @brief Function implementing the myTask04 thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_M3508_Task */
void M3508_Task(void *argument)
{
  /* USER CODE BEGIN M3508_Task */
    M3508[5].Expvel=MAXSPEED;
  /* Infinite loop */
  for(;;)
  {
    M3508_Motor_Position_Ctrl();
    M3508_Motor_Speed_Ctrl();
    M3508_Motor_Send_Current();
    M3508_Motor_Send_Current_h();
    osDelay(1);
  }
  /* USER CODE END M3508_Task */
}

/* USER CODE BEGIN Header_speedtask */
/**
* @brief Function implementing the Speedtask thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_speedtask */
void speedtask(void *argument)
{
  /* USER CODE BEGIN speedtask */
  
  /* Infinite loop */
  for(;;)
  {
    /*****************解析通道6，使用SA控制******************/
    switch (SBUS_Analyse_Switch(SBUS.CH5))
    {
        case -1:
        break;
      
        /*SA拨到最下面*/
        case 0: 
            /*开启手动校准yaw和力度*/
            jiaozhun_flag = 1;
        break;
      
        /*SA拨到中间*/
        case 1:
           jiaozhun_flag = 0;
        break;
      
        /*SA拨到上面*/
        case 2:
           index = index_temp;//通过手柄切换到任意点
           jiaozhun_flag = 1;//开启校准位
        break;
     
     /*********************************************************/     
      }
   osDelay(1);
  }
  /* USER CODE END speedtask */
}

/* USER CODE BEGIN Header_sbustask */
/**
* @brief Function implementing the Sbustask thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_sbustask */
void sbustask(void *argument)
{
  /* USER CODE BEGIN sbustask */
  
  /******该任务块淘汰赛添加功能
  *******/

  /* Infinite loop */

  for (;;)
  {
    /*******时间相关变量定义*************/
    static uint32_t last_time = 0;
    uint32_t current_time = HAL_GetTick();
    float dt = (current_time - last_time) / 1000.0f;
    dt = (dt < 0.001f) ? 0.001f : dt;  // 防止除零
    last_time = current_time;
    /***********************************/
    /****************自身坐标系下用于加速度限幅的相关变量*****************/
    static float prev_Vx1 = 0, prev_Vy1 = 0, prev_Vw1 = 0;  
//    float delta_Vx = 0,delta_Vy = 0,delta_Vw = 0;
//    float max_delta_Vx = 0,max_delta_Vy = 0,max_delta_Vw = 0;
    /*********************************************************************/
    /****************世界坐标系下用于加速度限幅的相关变量*****************/
    float delta_Vx1 = 0,delta_Vy1 = 0,delta_Vw1 = 0;
    float max_delta_Vx1 = 0,max_delta_Vy1 = 0,max_delta_Vw1 = 0;
    /*********************************************************************/
    
    
    
    /**************************解析通道8，使用SF控制***********************/
    switch (SBUS_Analyse_Switch(SBUS.CH8))
    {

    case -1:
    break;
    
//    /******************************手动模式在自身坐标系下移动******************************/
//    case 0:
////      Robot.Robot_Contorl_Mode = CONTROL_MANUAL;
////      /*原始速度计算，通过拨动手柄赋值*/
////      /*拨杆键：前后为通道2，左右为通道1，转yaw为通道4*/
////      Robot.Robot_in_self.velocity_exp_Unsure.Vx = ROBOT_SPEED_MAX * (SBUS_Analyse_Percent(SBUS.CH1) - 0.5) * 2;
////      Robot.Robot_in_self.velocity_exp_Unsure.Vy = ROBOT_SPEED_MAX * (SBUS_Analyse_Percent(SBUS.CH2) - 0.5) * 2;
////      Robot.Robot_in_self.velocity_exp_Unsure.Vw = ROBOT_SPEED_YAW_MAX * (SBUS_Analyse_Percent_CH4(SBUS.CH4) - 0.5) * 20;
////      /************************************************************************************/
////      /************************************加速度限制**************************************/
////      //X方向
////      delta_Vx = Robot.Robot_in_self.velocity_exp_Unsure.Vx - prev_Vx1;
////      max_delta_Vx = (fabs(Robot.Robot_in_self.velocity_exp_Unsure.Vx) > fabs(prev_Vx1)) 
////                         ? MAX_ACCEL_UP_VX1 * dt : MAX_ACCEL_DOWN_VX1 * dt;
////      delta_Vx = fmaxf(fminf(delta_Vx, max_delta_Vx), -max_delta_Vx);
////      Robot.Robot_in_self.velocity_exp_Unsure.Vx = prev_Vx1 + delta_Vx;
////      
////      // Y方向
////      delta_Vy = Robot.Robot_in_self.velocity_exp_Unsure.Vy - prev_Vy1;
////      max_delta_Vy = (fabs(Robot.Robot_in_self.velocity_exp_Unsure.Vy) > fabs(prev_Vy1)) 
////                         ? MAX_ACCEL_UP_VY1 * dt : MAX_ACCEL_DOWN_VY1 * dt;
////      delta_Vy = fmaxf(fminf(delta_Vy, max_delta_Vy), -max_delta_Vy);
////      Robot.Robot_in_self.velocity_exp_Unsure.Vy = prev_Vy1 + delta_Vy;
////      
////      // 角速度
////      delta_Vw = Robot.Robot_in_self.velocity_exp_Unsure.Vw - prev_Vw1;
////      max_delta_Vw = (fabs(Robot.Robot_in_self.velocity_exp_Unsure.Vw) > fabs(prev_Vw1)) 
////                         ? MAX_ACCEL_UP_VW1 * dt : MAX_ACCEL_DOWN_VW1 * dt;
////      delta_Vw = fmaxf(fminf(delta_Vw, max_delta_Vw), -max_delta_Vw);
////      Robot.Robot_in_self.velocity_exp_Unsure.Vw = prev_Vw1 + delta_Vw;
////      /**************************************************************************************/
////      // 最终速度限幅
////      Robot.Robot_in_self.velocity_exp.Vx = Limit_Speed(Robot.Robot_in_self.velocity_exp_Unsure.Vx, ROBOT_SPEED_MAX);
////      Robot.Robot_in_self.velocity_exp.Vy = Limit_Speed(Robot.Robot_in_self.velocity_exp_Unsure.Vy, ROBOT_SPEED_MAX);
////      Robot.Robot_in_self.velocity_exp.Vw = Limit_Speed(Robot.Robot_in_self.velocity_exp_Unsure.Vw, ROBOT_SPEED_YAW_MAX);
////      prev_Vx1 = Robot.Robot_in_self.velocity_exp.Vx;
////      prev_Vy1 = Robot.Robot_in_self.velocity_exp.Vy;
////      prev_Vw1 = Robot.Robot_in_self.velocity_exp.Vw;
////    break;
////    /******************************************************************************************/
    /*********************************手动在世界坐标系下移动***********************************/
    case 0:
      Robot.Robot_Contorl_Mode = CONTROL_WORLD;
       /*原始速度计算，通过拨动手柄赋值*/
      Robot.Robot_in_world.velocity_exp_Unsure.Vx = ROBOT_SPEED_MAX  * (SBUS_Analyse_Percent(SBUS.CH1) - 0.5) * 2;
      Robot.Robot_in_world.velocity_exp_Unsure.Vy = ROBOT_SPEED_MAX *(SBUS_Analyse_Percent(SBUS.CH2) - 0.5) * 2;
      Robot.Robot_in_world.velocity_exp_Unsure.Vw = ROBOT_SPEED_YAW_MAX * (SBUS_Analyse_Percent_CH4(SBUS.CH4) - 0.5) * 20;
      /************************************************************************************/
      /************************************加速度限制**************************************/
      //X方向
      delta_Vx1 = Robot.Robot_in_world.velocity_exp_Unsure.Vx - prev_Vx1;
      max_delta_Vx1 = (fabs(Robot.Robot_in_world.velocity_exp_Unsure.Vx) > fabs(prev_Vx1)) 
                   ? MAX_ACCEL_UP_VX1 * dt : MAX_ACCEL_DOWN_VX1 * dt;
      delta_Vx1 = fmaxf(fminf(delta_Vx1, max_delta_Vx1), -max_delta_Vx1);
      Robot.Robot_in_world.velocity_exp_Unsure.Vx = prev_Vx1 + delta_Vx1;
      // Y方向
      delta_Vy1 = Robot.Robot_in_world.velocity_exp_Unsure.Vy - prev_Vy1;
      max_delta_Vy1 = (fabs(Robot.Robot_in_world.velocity_exp_Unsure.Vy) > fabs(prev_Vy1)) 
                   ? MAX_ACCEL_UP_VY1 * dt : MAX_ACCEL_DOWN_VY1 * dt;
      delta_Vy1 = fmaxf(fminf(delta_Vy1, max_delta_Vy1), -max_delta_Vy1);
      Robot.Robot_in_world.velocity_exp_Unsure.Vy = prev_Vy1 + delta_Vy1;
      // 角速度
      delta_Vw1 = Robot.Robot_in_world.velocity_exp_Unsure.Vw - prev_Vw1;
      max_delta_Vw1 = (fabs(Robot.Robot_in_world.velocity_exp_Unsure.Vw) > fabs(prev_Vw1)) 
                   ? MAX_ACCEL_UP_VW1 * dt : MAX_ACCEL_DOWN_VW1 * dt;
      delta_Vw1 = fmaxf(fminf(delta_Vw1, max_delta_Vw1), -max_delta_Vw1);
      Robot.Robot_in_world.velocity_exp_Unsure.Vw = prev_Vw1 + delta_Vw1;
      /**************************************************************************************/
      // 最终速度限幅
      Robot.Robot_in_world.velocity_exp.Vx = Limit_Speed(Robot.Robot_in_world.velocity_exp_Unsure.Vx, ROBOT_SPEED_MAX);
      Robot.Robot_in_world.velocity_exp.Vy = Limit_Speed(Robot.Robot_in_world.velocity_exp_Unsure.Vy, ROBOT_SPEED_MAX);
      Robot.Robot_in_world.velocity_exp.Vw = Limit_Speed(Robot.Robot_in_world.velocity_exp_Unsure.Vw, ROBOT_SPEED_YAW_MAX);
      prev_Vx1 = Robot.Robot_in_world.velocity_exp.Vx;
      prev_Vy1 = Robot.Robot_in_world.velocity_exp.Vy;
      prev_Vw1 = Robot.Robot_in_world.velocity_exp.Vw;
    break;
    /**********************************************************************************************/
    /*************************************自动在世界坐标系下移动***********************************/
    case 2:
      Robot.Robot_Contorl_Mode = CONTROL_AUTO;
      Robot_Move_To_Target_Position(dingdian_x[index],dingdian_y[index],dingdian_yaw[index]);
//      /**************刚切自动挡开始运球***************/
//      if(steps == 0)
//      {
//          yunqiu();
//          steps = 1;
//      }
//      /****开始跑到某个固定点位并使用激光进行定位****/
//      if(steps == 1)
//      {
//          Robot_Move_To_Target_Position(dingdian_x[index],dingdian_y[index],dingdian_yaw[index]);/*先用码盘定位跑到一个点使激光有数再切回激光定位*/
//      }

    break;
    }
    osDelay(10);
  }
  
  /* USER CODE END sbustask */
}

/* USER CODE BEGIN Header_ads1115 */
/**
* @brief Function implementing the ADS1115 thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_ads1115 */
void ads1115(void *argument)
{
  /* USER CODE BEGIN ads1115 */
   ADS1115_UserConfig_SingleConver(&ADS1115_ADDR_GND,ADS1115_ADDRESS_GND);
   ADS1115_UserConfig_SingleConver(&ADS1115_ADDR_VDD,ADS1115_ADDRESS_VDD);
   ADS1115_UserConfig_SingleConver(&ADS1115_ADDR_SDA,ADS1115_ADDRESS_SDA);
   ADS1115_UserConfig_SingleConver(&ADS1115_ADDR_SCL,ADS1115_ADDRESS_SCL);
   
  ///Continu
  //ADS1115_UserConfig_SingleConver(&ADS1115_ADDR_GND,ADS1115_ADDRESS_GND);
  //ADS1115_UserConfig_SingleConver(&ADS1115_ADDR_VDD,ADS1115_ADDRESS_VDD);
  //uint8_t i = 0;
  /* Infinite loop */
  for(;;)
  {
     ADS1115_GetVoltage(&ADS1115_ADDR_VDD,ADS1115_CHANNEL1);
     ADS1115_GetVoltage(&ADS1115_ADDR_GND,ADS1115_CHANNEL1);
   // ADS1115_GetVoltage(&ADS1115_ADDR_SDA,ADS1115_CHANNEL1);

   // ADS1115_GetVoltage(&ADS1115_ADDR_SCL,ADS1115_CHANNEL1);
			
    // ADS1115_GetVoltage(&ADS1115_ADDR_VDD,ADS1115_ADDR_VDD.CHANNEL);
    //ADS1115_RefreshAllChannel(&ADS1115_ADDR_VDD);
     
     
    /*************激光定位坐标x,y**************/
    laser_x=laser_to_actual(ADS1115_ADDR_VDD.ADS1115_RawData[1],4);
    laser_y=laser_to_actual(ADS1115_ADDR_GND.ADS1115_RawData[1],1);
    /*最后一个定点使用激光定位，拨杆拨到最下方*/
    if(steps == 2)
    {
        Robot.Robot_in_self.position_now.x = laser_x/100;
        Robot.Robot_in_self.position_now.y = laser_y/100;
    }
    /******************************************/
    
    //ADS1115_ScanChannel(&ADS1115_ADDR_GND,ADS1115_CHANNEL1);
    //ADS1115_GetVoltage(&ADS1115_ADDR_GND,ADS1115_CHANNEL1);

    osDelay(50);
  }
  /* USER CODE END ads1115 */
}

/* USER CODE BEGIN Header_Change_yaw */
/**
* @brief Function implementing the change_yaw thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_Change_yaw */
void Change_yaw(void *argument)
{
  /* USER CODE BEGIN Change_yaw */
  
  /*
  *该任务块用于改变yaw值
  */
  uint8_t current_CH9_state = 0;
  uint8_t last_CH9_state = 0;
  /* Infinite loop */
  for(;;)
  {
    if(jiaozhun_flag == 1)
    {
        switch(SBUS_Analyse_Switch(SBUS.CH9))
        {
            /*******拨杆拨动到上面，yaw的值往左偏0.02rad********/
            case 0:
              current_CH9_state = 0;
              if(current_CH9_state != last_CH9_state)
              {
                   osDelay(10);/*消抖*/
                   if(current_CH9_state != last_CH9_state)
                   {
                        last_CH9_state = 0;
                        extra_angle = extra_angle - 1;
                   }
              }
            break;
            /***************************************************/
            
            /**************拨杆拨到中间，yaw的值不变************/
            case 1:
              current_CH9_state = 1;
              last_CH9_state = 1;
            break;
            /**************************************************/
            
            /********拨杆拨到下面，yaw的值往右偏0.02rad*********/
            case 2:
              current_CH9_state = 2;
              if(current_CH9_state != last_CH9_state)
              {
                   osDelay(10);/*消抖*/
                   if(current_CH9_state != last_CH9_state)
                   {
                        last_CH9_state = 2;
                        extra_angle = extra_angle + 1;
                   }
              }
            break;
        /***************************************************/
        }
    }
    osDelay(1);
  }
  /* USER CODE END Change_yaw */
}

/* USER CODE BEGIN Header_Change_N */
/**
* @brief Function implementing the change_N thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_Change_N */
void Change_N(void *argument)
{
  /* USER CODE BEGIN Change_N */
  
  /*
  *该任务块用于改变力矩
  */
  uint8_t current_CH7_state = 0;
  uint8_t last_CH7_state = 0;
  
  /* Infinite loop */
  for(;;)
  {
    if(jiaozhun_flag == 1)
    {
        switch(SBUS_Analyse_Switch(SBUS.CH7))
        {
            /*****拨杆拨动到上面，力矩的值增加+0.03N_M*****/
            case 0:
                current_CH7_state = 0;
                if(current_CH7_state != last_CH7_state)
                {
                    osDelay(10);
                    if(current_CH7_state != last_CH7_state)
                    {
                        extra_Tf = extra_Tf + 0.1;
                        last_CH7_state = 0;
                    }
                }
            break;
            /*********************************************/
            
            /**********拨杆拨到中间，力矩的值不变*********/
            case 1:
              current_CH7_state = 1;
              last_CH7_state = 1;
            break;
            /*********************************************/
            
            /******拨杆拨到下面，力矩的值减小-0.1N_M******/
            case 2:
                current_CH7_state = 2;
                if(current_CH7_state != last_CH7_state)
                {
                    osDelay(10);
                    if(current_CH7_state != last_CH7_state)
                    {
                        extra_Tf = extra_Tf - 0.1;
                        last_CH7_state = 2;
                    }
                }
            break;
            /*********************************************/
        }
    
    
    }

    osDelay(1);
  }
  /* USER CODE END Change_N */
}

/* USER CODE BEGIN Header_Ak7010_reinit */
/**
* @brief Function implementing the ak7010_reinit thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_Ak7010_reinit */
void Ak7010_reinit(void *argument)
{
  /* USER CODE BEGIN Ak7010_reinit */
  
  /*
   *该任务块用于复活电机Ak7010
   */
  int last_status = 0;
  /* Infinite loop */
  for(;;)
  {
//    int switchStatus = SBUS_Analyse_Switch(SBUS.CH14); /*解析通道14SH状态状态*/
//    /****此时拨杆拨到上方****/
//    if(switchStatus == 2)
//    {
//        osDelay(10);
//        if(switchStatus != last_status)
//        {
//           /******置1标志位******/
//            reinit_flag = 1;
//            last_status = 2;
//        }
//    }
//    
//    /****此时拨杆回到下方****/
//    if(switchStatus == 0)
//    {
//        osDelay(10);
//        if(switchStatus == 0)
//        {
//           /******清空标志位******/
//            reinit_flag = 0;
//        }
//        last_status = 0;
//    }
    osDelay(1);
  }
  /* USER CODE END Ak7010_reinit */
}

/* USER CODE BEGIN Header_ch14_to_hand_yunqiu */
/**
* @brief Function implementing the CH14_to_hand_yu thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_ch14_to_hand_yunqiu */
void ch14_to_hand_yunqiu(void *argument)
{
  /* USER CODE BEGIN ch14_to_hand_yunqiu */
  
  /*******拨杆SH通过CH14控制手动运球*******/
  
  /* Infinite loop */
  for(;;)
  {
      if(SBUS_Analyse_Switch(SBUS.CH14)== 2 && yunqiu_flag == 1 && M3508[5].Exparg== MID_POSITION)
      {
          
          yunqiu_flag = 0;
          HAL_GPIO_WritePin (hand,hand_PIN,GPIO_PIN_SET);//松开夹爪
          //osDelay(50); 
          HAL_GPIO_WritePin (leg,leg_PIN,GPIO_PIN_SET);//气缸击打运球
          osDelay(200);
          HAL_GPIO_WritePin (leg,leg_PIN,GPIO_PIN_RESET);//气缸回收

          claw_flag = 1;
          __HAL_TIM_SET_COUNTER(&htim5,0);
          HAL_TIM_Base_Start(&htim5);//
          
      }
      if(SBUS_Analyse_Switch(SBUS.CH14) == 0)
      {
          yunqiu_flag = 1;
      }
    
    osDelay(1);
  }
  /* USER CODE END ch14_to_hand_yunqiu */
}

/* USER CODE BEGIN Header_Change_angle */
/**
* @brief Function implementing the change_angle thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_Change_angle */
void Change_angle(void *argument)
{
  /* USER CODE BEGIN Change_angle */
  
  /*该任务块用旋钮调节出手角度*/
  
  /* Infinite loop */
  for(;;)
  {
//    if(jiaozhun_flag == 1)
//    {
//        switch(SBUS_Analyse_Switch(SBUS.CH10))
//        {
//            case 0:/*角度加大0.2*/
//              current_CH10_state = 0;
//              if(current_CH10_state != last_CH10_state)
//              {
//                osDelay(10);
//                if(current_CH10_state != last_CH10_state)
//                {
//                    extra_angle = extra_angle + 0.2;
//                }
//                last_CH10_state = 0;
//              }
//            break;
//            
//            case 1:
//              current_CH10_state = 1;
//              last_CH10_state = 1;
//            break;
//            
//            case 2:/*角度减小0.2*/
//              current_CH10_state = 2;
//              if(current_CH10_state != last_CH10_state)
//              {
//                osDelay(10);
//                if(current_CH10_state != last_CH10_state)
//                {
//                    extra_angle = extra_angle - 0.2;
//                }
//                last_CH10_state = 2;
//              }
//            break;
//        }
 //  }
    
    osDelay(1);
  }
  /* USER CODE END Change_angle */
}

/* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */
void Connect_to_F405(void)
{
    for(int i = 0;i < 10;i++)
    {
      Tx_buffer[0] = 0x25;
      Tx_buffer[1] = 0x05;
      Tx_buffer[16] = 0x55;
      SendBuffer.float_Buffer[0] = Robot.Robot_in_self.position_now.x;
      SendBuffer.float_Buffer[1] = Robot.Robot_in_self.position_now.y;
      SendBuffer.float_Buffer[2] = Robot.Robot_in_self.position_now.yaw;
      Tx_buffer[2] = gear;
      Tx_buffer[3] = AK7010_flag;
      Tx_buffer[4] = SendBuffer.bytes[0];
      Tx_buffer[5] = SendBuffer.bytes[1];
      Tx_buffer[6] = SendBuffer.bytes[2];
      Tx_buffer[7] = SendBuffer.bytes[3];
      Tx_buffer[8] = SendBuffer.bytes[4];
      Tx_buffer[9] = SendBuffer.bytes[5];
      Tx_buffer[10] = SendBuffer.bytes[6];
      Tx_buffer[11] = SendBuffer.bytes[7];
      Tx_buffer[12] = SendBuffer.bytes[8];
      Tx_buffer[13] = SendBuffer.bytes[9];
      Tx_buffer[14] = SendBuffer.bytes[10];
      Tx_buffer[15] = SendBuffer.bytes[11];     
      HAL_UART_Transmit(&huart4,Tx_buffer,17,0xfff);
    }
}

void yunqiu(void)
{
     HAL_GPIO_WritePin (hand,hand_PIN,GPIO_PIN_SET);//松开夹爪
          //osDelay(50);
     HAL_GPIO_WritePin (leg,leg_PIN,GPIO_PIN_SET);//气缸击打运球
          
     osDelay(200);
     HAL_GPIO_WritePin (leg,leg_PIN,GPIO_PIN_RESET);//气缸回收
     
     /******开启光电门检测******/
     claw_flag = 1;
     __HAL_TIM_SET_COUNTER(&htim5,0);
     HAL_TIM_Base_Start(&htim5);
     /**************************/
     
     osDelay(1100);
     HAL_GPIO_WritePin (hand,hand_PIN,GPIO_PIN_SET);//松开夹爪
          
     // osDelay(50);//30  25  
     HAL_GPIO_WritePin (leg,leg_PIN,GPIO_PIN_SET);//气缸击打运球
          
     osDelay(200);
     HAL_GPIO_WritePin (leg,leg_PIN,GPIO_PIN_RESET);//气缸回收

     /******开启光电门检测******/          
     claw_flag = 1;
     __HAL_TIM_SET_COUNTER(&htim5,0);
     HAL_TIM_Base_Start(&htim5);
     /**************************/
}
/* USER CODE END Application */

